package lightcar;

import java.awt.Graphics;


public class Car
{
    private double width,length;
    private double k11, k12, k21, k22;
    private double direction; //0 == horizontal
    private Point wheel1, wheel2;
    
    Car()
    {
        length = 50;
        width = 100;
        wheel1 = new Point(0,0);
        wheel2 = new Point(0,0);
        k11 = 1; k12 = 1; k21 = 1; k22 = 1;
        direction = 0;
    }
    
    Car(Point corner, double direction, double k11, double k12, double k21, double k22)
    {
        this.direction = direction;
        length = 50;
        width = 100;
        wheel1 = new Point(corner.x, corner.y);
        wheel2 = new Point(corner.x+length*Math.sin(Math.toRadians(direction)),corner.y+length*Math.cos(Math.toRadians(direction)));
        this.k11 = k11;
        this.k12 = k12;
        this.k21 = k21;
        this.k22 = k22;
        System.out.println("Car created at: " + wheel1.x + "," + wheel1.y);
    }
    
    public void draw(Graphics g)
    {
        double sensor1x=wheel1.x+width*Math.cos(Math.toRadians(direction));
        double sensor1y=wheel1.y-width*Math.sin(Math.toRadians(direction));
        double sensor2x=wheel2.x+width*Math.cos(Math.toRadians(direction));
        double sensor2y=wheel2.y-width*Math.sin(Math.toRadians(direction));
        int[] xPoints = {(int)sensor2x,(int)sensor1x,(int)wheel1.x,(int)wheel2.x};
        int[] yPoints = {(int)sensor2y,(int)sensor1y,(int)wheel1.y,(int)wheel2.y};
        g.drawPolygon(xPoints,yPoints,4);
        //System.out.println(sensor1);
        //System.out.println(sensor2);
        
        //Wheels
        int vertical_length = (int)(0.23*length);
        int horizontal_length = (int) (.25*width);
        //int tail = (int) (.65 * horizontal_length);
        //Wheel 1        
        g.drawLine((int) wheel1.x, (int) wheel1.y, (int)(wheel1.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int)(wheel1.y-horizontal_length*Math.sin(Math.toRadians(direction))));
        double pythresult = pyth(horizontal_length,vertical_length);       
        double cosresult = cos(horizontal_length, pythresult, vertical_length);       
        cosresult = Math.toDegrees(cosresult);         
        g.drawLine((int)(wheel1.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int)(wheel1.y-horizontal_length*Math.sin(Math.toRadians(direction))),(int) (wheel1.x+pythresult*Math.cos(Math.toRadians(direction+cosresult))), (int)(wheel1.y-pythresult*Math.sin(Math.toRadians(direction+cosresult))));
        g.drawLine((int) (wheel1.x+pythresult*Math.cos(Math.toRadians(direction+cosresult))), (int)(wheel1.y-pythresult*Math.sin(Math.toRadians(direction+cosresult))), (int) (wheel1.x), (int) wheel1.y);
        
        //System.out.println("WHEEL: " + (wheel1.x+horizontal_length*Math.cos(Math.toRadians(direction))));
        //g.drawLine((int) (wheel1.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int) (wheel1.y-horizontal_length*Math.sin(Math.toRadians(direction))), (int) (wheel1.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int) ((wheel1.y - vertical_length - horizontal_length*Math.sin(Math.toRadians(direction)))));
        //g.drawLine((int) (wheel1.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int) ((wheel1.y - vertical_length - horizontal_length*Math.sin(Math.toRadians(direction)))), (int)(wheel1.x - tail*Math.cos(Math.toRadians(direction))), (int) (wheel1.y - vertical_length-tail*Math.sin(Math.toRadians(direction))));
       // g.drawLine((int)wheel1.x - tail, (int) wheel1.y - vertical_length, (int)wheel1.x - tail, (int) wheel1.y);
       // g.drawLine((int)wheel1.x - tail, (int) wheel1.y, (int) wheel1.x, (int) wheel1.y);
        
       
        //Wheel 2
        g.drawLine((int) wheel2.x, (int) wheel2.y, (int)(wheel2.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int)(wheel2.y-horizontal_length*Math.sin(Math.toRadians(direction))));
        g.drawLine((int)(wheel2.x+horizontal_length*Math.cos(Math.toRadians(direction))), (int)(wheel2.y-horizontal_length*Math.sin(Math.toRadians(direction))),(int) (wheel2.x+pythresult*Math.cos(Math.toRadians(direction-cosresult))), (int)(wheel2.y-pythresult*Math.sin(Math.toRadians(direction-cosresult))));
        g.drawLine((int) (wheel2.x+pythresult*Math.cos(Math.toRadians(direction-cosresult))), (int)(wheel2.y-pythresult*Math.sin(Math.toRadians(direction-cosresult))), (int)(wheel2.x), (int)(wheel2.y));
       //g.drawLine((int) wheel2.x+horizontal_length, (int) wheel2.y, (int) wheel2.x+horizontal_length, (int) wheel2.y + vertical_length);
      //  g.drawLine((int) wheel2.x+horizontal_length, (int) wheel2.y + vertical_length, (int)wheel2.x - tail, (int) wheel2.y + vertical_length);
      //  g.drawLine((int)wheel2.x - tail, (int) wheel2.y + vertical_length, (int)wheel2.x - tail, (int) wheel2.y);
     //   g.drawLine((int)wheel2.x - tail, (int) wheel2.y, (int) wheel2.x, (int) wheel2.y);
        
        
        
        //Sensor 1
        double sSize = (length/4);
        g.drawArc((int)(sensor1x-sSize/2*(1-Math.cos(Math.toRadians(direction)))), (int)(sensor1y - sSize/2*(1+Math.sin(Math.toRadians(direction)))), (int)sSize, (int)sSize, (int)(direction + 90), 180);
       
        //Sensor 2
        g.drawArc((int)(sensor2x-sSize/2*(1-Math.cos(Math.toRadians(direction)))), (int)(sensor2y - sSize/2*(1+Math.sin(Math.toRadians(direction)))), (int)sSize,(int)sSize, (int)(direction + 90), 180);
    }
    
    public double cos(double a, double b, double c)
    {
        double sqrt = 0;
        sqrt = Math.sqrt(((b*b)+ (c*c) - (a*a))/(2*b*c));
        return Math.toDegrees(Math.acos(sqrt));        
    }
    
    public double pyth(double a, double b)
    {
        return Math.sqrt((a*a) + (b*b));
    }
    public void move(double left, double right, double time)
    {
        //System.out.println("left: " + left);
        //System.out.println("right: " + right);
        double w1=k11*left+k12*right;
        double w2=k21*left+k22*right;
        double dteta=0;
        double r;
        //System.out.println("w1: " + w1);
        //System.out.println("w2: " + w2);
        //w1=5;
        //w2=2;
        if (w2>w1)  {
            r=w1*width/(w2-w1);
            dteta=w2*time*360/(2*Math.PI*(r+width));
            double axisX = wheel1.x-r*Math.sin(Math.toRadians(direction));
            double axisY = wheel1.y-r*Math.cos(Math.toRadians(direction));
            direction=(direction+dteta)%360;
            wheel1.x=axisX+r*Math.sin(Math.toRadians(direction));
            wheel1.y=axisY+r*Math.cos(Math.toRadians(direction));
            wheel2.x=wheel1.x+length*Math.sin(Math.toRadians(direction));
            wheel2.y=wheel1.y+length*Math.cos(Math.toRadians(direction));
        }
        else if (w1>w2) {
            r=w2*width/(w1-w2);
            dteta=-w1*time*360/(2*Math.PI*(r+width));
            double axisX = wheel2.x+r*Math.sin(Math.toRadians(direction));
            double axisY = wheel2.y+r*Math.cos(Math.toRadians(direction));
            direction=(direction+dteta)%360;
            wheel2.x=axisX-r*Math.sin(Math.toRadians(direction));
            wheel2.y=axisY-r*Math.cos(Math.toRadians(direction));
            wheel1.x=wheel2.x-length*Math.sin(Math.toRadians(direction));
            wheel1.y=wheel2.y-length*Math.cos(Math.toRadians(direction));
        }
        else {
            wheel1.x=wheel1.x+Math.cos(Math.toRadians(direction))*w1*time;
            wheel1.y=wheel1.y+Math.sin(Math.toRadians(direction))*w1*time;
            wheel2.x=wheel2.x+Math.cos(Math.toRadians(direction))*w2*time;
            wheel2.y=wheel2.y+Math.sin(Math.toRadians(direction))*w2*time;
        }
        //System.out.println("direction:" + direction);
        
        //System.out.println("Wheel1" + wheel1);
        //System.out.println("Wheel2" + wheel2);
    }
    
    public Point leftSensor()
    {
        //Returns the current position of the left sensor
        double sensor1x=wheel1.x+width*Math.cos(Math.toRadians(direction));
        double sensor1y=wheel1.y-width*Math.sin(Math.toRadians(direction));
        return new Point(sensor1x,sensor1y);
    }
    
    public Point rightSensor()
    {
        //Returns the current position of the right sensor
        double sensor2x=wheel2.x+width*Math.cos(Math.toRadians(direction));
        double sensor2y=wheel2.y-width*Math.sin(Math.toRadians(direction));
        return new Point(sensor2x,sensor2y);
    }
    
    /*public Point closestLightSource()
    {
        double minLeft = 1000;
        double minRight = 1000;
        Point minLeftPoint = new Point(0,0);
        Point minRightPoint = new Point(0,0);
        for(int i = 0; i < World.lights.size(); i++)
        {
            double tempLeft = World.dist(sensor1, World.lights.get(i).getLocation());
            double tempRight = World.dist(sensor2, World.lights.get(i).getLocation());
            if (tempLeft < minLeft)
            {
                minLeft = tempLeft;
                minLeftPoint = World.lights.get(i).getLocation();
            }
            if(tempRight < minRight)
            {
                minRight = tempRight; 
                minRightPoint = World.lights.get(i).getLocation();
            }    
        }
        if(minLeft < minRight)
            return minLeftPoint;
        else
            return minRightPoint;
        
    }*/
}
